#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <pantilt_control/PtuGotoAction.h>
#include <geometry_msgs/PointStamped.h>
#include <std_msgs/String.h>
#include <dynamixel_msgs/JointState.h>
#include <std_msgs/Float64.h>
#define PAN_THRESHOLD 0.02  // .02   .1
#include<math.h>
#define pi M_PI
#include <iostream>
#include<pthread.h>

class dxlAction
{
protected:

    ros::NodeHandle n;
    ros::Rate sleep_rate;
    actionlib::SimpleActionServer<pantilt_control::PtuGotoAction> as;
    std::string action_name;
    pantilt_control::PtuGotoFeedback feedback;
    pantilt_control::PtuGotoResult result;
    double max_pan;
    double min_pan;
    double max_tilt;
    double min_tilt;
    double current_pan;
    double current_tilt;
    sensor_msgs::JointState current_joint_state;
    bool success;
    ros::Publisher pub_pan,pub_tilt;
    ros::Subscriber sub;
    bool isTimeout;

    pthread_mutex_t mutex;
    pthread_mutex_t timer_mutex;

public:

    dxlAction(std::string name) :
        sleep_rate(10),
        as(n, "/SetPTUState", boost::bind(&dxlAction::executeCB, this, _1), false),
        action_name(name),isTimeout(false)
    {
        as.start();
        max_pan=90;
        min_pan=-90;
        max_tilt=90;
        min_tilt=-90;
        success=false;
        pub_pan = n.advertise<std_msgs::Float64>("/dynamixel_controller/head_pan_controller/command",10);
        pub_tilt = n.advertise<std_msgs::Float64>("/dynamixel_controller/head_tilt_controller/command",10);
        sub = n.subscribe("/joint_states", 1,&dxlAction::jointStateCB,this);
    }


    ~dxlAction(void)
    {
    }

    void executeCB(const pantilt_control::PtuGotoGoalConstPtr &goal)
    {

        ROS_INFO("Request received, %f %f",goal->pan,goal->tilt);
        isTimeout = false;
        double goal_pan,goal_tilt;
        goal_pan=(goal->pan*pi)/180;
        goal_tilt=(goal->tilt*pi)/180;
        std_msgs::Float64 pan_value,tilt_value;


        if(goal->pan < min_pan || goal->tilt < min_tilt  || goal->tilt > max_tilt || goal->pan > max_pan)
        {
            ROS_ERROR("%s: out of range!", action_name.c_str());
            as.setAborted();
        }
        else
        {
            pan_value.data=goal_pan;
            tilt_value.data=goal_tilt;
            pub_pan.publish(pan_value);
            pub_tilt.publish(tilt_value);
            ros::Timer time=n.createTimer(ros::Duration(8,0),&dxlAction::timerCB,this);
            double cur_pan,cur_tilt;
            bool time_out;
            do
            {
                pthread_mutex_lock(&mutex);

                cur_pan=current_pan;
                cur_tilt=current_tilt;

                feedback.state = current_joint_state;

                pthread_mutex_unlock(&mutex);


                pthread_mutex_lock(&timer_mutex);

                time_out=isTimeout;

                pthread_mutex_unlock(&timer_mutex);

                as.publishFeedback(feedback);
                sleep_rate.sleep();
                ROS_INFO("goal_pan= %f... cr_pan= %f",goal_pan,cur_pan);
                ROS_INFO("goal_tilt= %f... cr_tilt= %f",goal_tilt,cur_tilt);
            }
            while( ( ( fabs(goal_pan - cur_pan) > PAN_THRESHOLD) ||
                     ( fabs(goal_tilt - cur_tilt) > PAN_THRESHOLD) ) &&
                   ros::ok() && !time_out );

            time.stop();

            if(!time_out)
            {
                success = true;
                ROS_INFO("SUCCESS");
            }
            else
            {
                success = false;
                ROS_INFO("Failed");
            }

        }

        if(success)
        {
            ROS_INFO("%s: Succeeded", action_name.c_str());
            result.state=feedback.state;
            as.setSucceeded(result);
        }
        else
        {
            ROS_INFO("%s: Aborted!",action_name.c_str());
            as.setAborted(result);
        }

    }
    void jointStateCB(const sensor_msgs::JointStateConstPtr& msg)
    {
        pthread_mutex_lock(&mutex);

        //        std::cout<<(*msg)<<std::endl;


        current_pan = msg->position[0];
        current_tilt = msg->position[1];
        current_joint_state = *msg;
        pthread_mutex_unlock(&mutex);

    }


    void timerCB(const ros::TimerEvent&)
    {

        ROS_INFO("ERROR!  TIME OUT!");
        pthread_mutex_lock(&timer_mutex);
        isTimeout = true;
        pthread_mutex_unlock(&timer_mutex);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ptu");
    dxlAction ptu_dynamixel("ptu");
    ros::AsyncSpinner spinner(0);
    spinner.start();
    ros::waitForShutdown();
    return 0;
}
